home *** CD-ROM | disk | FTP | other *** search
/ Resource for Source: C/C++ / Resource for Source - C-C++.iso / codelib7 / v_09_11 / 9n11052a < prev    next >
Encoding:
Text File  |  1995-11-01  |  12.2 KB  |  354 lines
  1. /* -- simpwalk.h --
  2.  */
  3. #define LEGDOWN_CYCLE_TIME                      50
  4. #define ALPHA_ADVANCE_CYCLE_TIME        10
  5. #define GAIT_CYCLE_TIME                         500
  6.  
  7. typedef enum { ALPHA_DECR = -1, NEUTRAL, ALPHA_INCR } POSITION;
  8. #define BETA_UP         127
  9. #define BETA_DOWN       -128
  10.  
  11. #define IS_BETA_UP(x)   !IS_BETA_DOWN(x)
  12. #define IS_BETA_DOWN(x) ((x) < 0)
  13.  
  14. typedef struct
  15.         {
  16.         char    *inName;
  17.         char    *outName;
  18.         int             servoName;
  19.         } IO_NAME;
  20.  
  21. extern IO_NAME alphaData[], betaData[];
  22. void alphaPos(void), betaPos(void);
  23.  
  24. /* -- standup.c --
  25.  */
  26. #include <stdio.h>
  27. #include "exec.h"
  28. #include "simpwalk.h"
  29.  
  30. #define alpha0Servo 0
  31. #define alpha1Servo 1
  32. #define alpha2Servo 2
  33. #define alpha3Servo 3
  34. #define alpha4Servo 4
  35. #define alpha5Servo 5
  36. #define beta0Servo  6
  37. #define beta1Servo  7
  38. #define beta2Servo  8
  39. #define beta3Servo  9
  40. #define beta4Servo  10
  41. #define beta5Servo  11
  42.  
  43. IO_NAME alphaData[] = {
  44.         { "alpha0Pos.in", "alpha0Pos.out", alpha0Servo },
  45.         { "alpha1Pos.in", "alpha1Pos.out", alpha1Servo },
  46.         { "alpha2Pos.in", "alpha2Pos.out", alpha2Servo },
  47.         { "alpha3Pos.in", "alpha3Pos.out", alpha3Servo },
  48.         { "alpha4Pos.in", "alpha4Pos.out", alpha4Servo },
  49.         { "alpha5Pos.in", "alpha5Pos.out", alpha5Servo }
  50.         };
  51.  
  52. IO_NAME betaData[] = {
  53.         { "beta0Pos.in", "beta0Pos.out", beta0Servo },
  54.         { "beta1Pos.in", "beta1Pos.out", beta1Servo },
  55.         { "beta2Pos.in", "beta2Pos.out", beta2Servo },
  56.         { "beta3Pos.in", "beta3Pos.out", beta3Servo },
  57.         { "beta4Pos.in", "beta4Pos.out", beta4Servo },
  58.         { "beta5Pos.in", "beta5Pos.out", beta5Servo }
  59.         };
  60.  
  61. /*      alpha position.  One per leg.
  62.  */
  63. void alphaPos(void)
  64.         {
  65.         IO_NAME *pData = &alphaData[ExecArg0];
  66.         PORT portValue, position;
  67.         STRING_ATOM output = StringToAtom(pData->outName);
  68.  
  69.         MsgOpenPort(output, &portValue);
  70.         MsgWriteToPort(output, 
  71.                         servoWrite(pData->servoName, NEUTRAL));
  72.         MsgWatchPort(StringToAtom(pData->inName), &position);
  73.         while (MsgGetMsg())
  74.                 {
  75.                 MsgWriteToPort(output, 
  76.                                 servoWrite(pData->servoName, position));
  77.                 }
  78.         }
  79.  
  80. /*      beta position.  One per leg.
  81.  */
  82. void betaPos(void)
  83.         {
  84.         IO_NAME *pData = &betaData[ExecArg0];
  85.         EXEC_ID id, timerID;
  86.         PORT currentPos, portValue, position;
  87.         STRING_ATOM output = StringToAtom(pData->outName);
  88.  
  89.         MsgOpenPort(output, &portValue);
  90.         MsgWriteToPort(output, BETA_DOWN);
  91.  
  92.         servoWrite(pData->servoName, position = BETA_DOWN);
  93.         while (servoReadWrite(pData->servoName, position) != BETA_DOWN)
  94.                 ;
  95.         MsgWatchPort(StringToAtom(pData->inName), &position);
  96.         timerID = MsgWatchTimer(1);
  97.  
  98.         while (id = MsgGetMsg())
  99.                 {
  100.                 if (id == timerID)
  101.                         MsgWriteToPort(output,
  102. servoReadWrite(pData->servoName, position));
  103.                 else
  104.                         servoWrite(pData->servoName, position);
  105.                 }
  106.         }
  107.  
  108. /* -- simpwalk.c --
  109.  */
  110. #include <stdlib.h>
  111. #include "exec.h"
  112. #include "simpwalk.h"
  113.  
  114. /*      interface names
  115.  */
  116. IO_NAME legDownData[] = {
  117.         { "leg0Down.in", "leg0Down.out" },
  118.         { "leg1Down.in", "leg1Down.out" },
  119.         { "leg2Down.in", "leg2Down.out" },
  120.         { "leg3Down.in", "leg3Down.out" },
  121.         { "leg4Down.in", "leg4Down.out" },
  122.         { "leg5Down.in", "leg5Down.out" }
  123.         };
  124.  
  125. IO_NAME advanceData[] = {
  126.         { "alpha0Advance.in", "alpha0Advance.out" },
  127.         { "alpha1Advance.in", "alpha1Advance.out" },
  128.         { "alpha2Advance.in", "alpha2Advance.out" },
  129.         { "alpha3Advance.in", "alpha3Advance.out" },
  130.         { "alpha4Advance.in", "alpha4Advance.out" },
  131.         { "alpha5Advance.in", "alpha5Advance.out" }
  132.         };
  133.  
  134. IO_NAME uplegData[] = {
  135.         { "upleg0Trigger.in", "upleg0Trigger.out" },
  136.         { "upleg1Trigger.in", "upleg1Trigger.out" },
  137.         { "upleg2Trigger.in", "upleg2Trigger.out" },
  138.         { "upleg3Trigger.in", "upleg3Trigger.out" },
  139.         { "upleg4Trigger.in", "upleg4Trigger.out" },
  140.         { "upleg5Trigger.in", "upleg5Trigger.out" }
  141.         };
  142.  
  143. /*      put the leg down if it's up.  One per leg.
  144.  */
  145. void legDown(void)
  146.         {
  147.         IO_NAME *pData = &legDownData[ExecArg0];
  148.         PORT legPosition;
  149.         STRING_ATOM betaOut, output;
  150.  
  151.         betaOut = StringToAtom(betaData[ExecArg0].outName);
  152.         output = StringToAtom(pData->outName);
  153.         /*      connect our output to the beta position's input
  154.          */
  155.         MsgConnectPorts(output, StringToAtom(betaData[ExecArg0].inName));
  156.         MsgWatchTimer(LEGDOWN_CYCLE_TIME);
  157.  
  158.         while (MsgGetMsg())
  159.                 {
  160.                 MsgPeekPort(betaOut, &legPosition);
  161.                 if (IS_BETA_UP(legPosition))
  162.                         MsgWriteToPort(output, BETA_DOWN);
  163.                 }
  164.         }
  165.  
  166. /*      if the total alpha positions are positive, then move them back.
  167.  *      If it is negative, move them forward.  One for all legs.
  168.  */
  169. void alphaBalance(void)
  170.         {
  171.         STRING_ATOM alpha0, alpha1, alpha2, alpha3, alpha4, alpha5;
  172.         STRING_ATOM alphaOut;
  173.         PORT totalAlphaPosition;
  174.  
  175.         alpha0 = StringToAtom(alphaData[0].outName);
  176.         alpha1 = StringToAtom(alphaData[1].outName);
  177.         alpha2 = StringToAtom(alphaData[2].outName);
  178.         alpha3 = StringToAtom(alphaData[3].outName);
  179.         alpha4 = StringToAtom(alphaData[4].outName);
  180.         alpha5 = StringToAtom(alphaData[5].outName);
  181.         alphaOut = StringToAtom("alphaBalance.out");
  182.         /*      connect our output to the alpha motors' input
  183.          */
  184.         MsgConnectPorts(alphaOut, StringToAtom(alphaData[0].inName));
  185.         MsgConnectPorts(alphaOut, StringToAtom(alphaData[1].inName));
  186.         MsgConnectPorts(alphaOut, StringToAtom(alphaData[2].inName));
  187.         MsgConnectPorts(alphaOut, StringToAtom(alphaData[3].inName));
  188.         MsgConnectPorts(alphaOut, StringToAtom(alphaData[4].inName));
  189.         MsgConnectPorts(alphaOut, StringToAtom(alphaData[5].inName));
  190.  
  191.         MsgWatchTimer(ALPHA_ADVANCE_CYCLE_TIME);
  192.         while (MsgGetMsg())
  193.                 {
  194.                 totalAlphaPosition =
  195.                         MsgPeekPort(alpha0, NULL) +
  196.                         MsgPeekPort(alpha1, NULL) +
  197.                         MsgPeekPort(alpha2, NULL) +
  198.                         MsgPeekPort(alpha3, NULL) +
  199.                         MsgPeekPort(alpha4, NULL) +
  200.                         MsgPeekPort(alpha5, NULL);
  201.                 if (totalAlphaPosition < 0)
  202.                         MsgWriteToPort(alphaOut, ALPHA_INCR);
  203.                 else if (0 < totalAlphaPosition)
  204.                         MsgWriteToPort(alphaOut, ALPHA_DECR);
  205.                 }
  206.         }
  207.  
  208. /*      if the leg is up, then move leg forward.  One per leg.
  209.  */
  210. void alphaAdvance(void)
  211.         {
  212.         PORT betaPosition;
  213.         STRING_ATOM input, output, alphaPosInput;
  214.  
  215.         /*      wakes up whenever the beta position changes
  216.          */
  217.         input = StringToAtom(advanceData[ExecArg0].inName);
  218.         MsgWatchPort(input, &betaPosition);
  219.  
  220.         alphaPosInput = StringToAtom(alphaData[ExecArg0].inName);
  221.         output = StringToAtom(advanceData[ExecArg0].outName);
  222.  
  223.         /*      change alpha position input to connect to our output
  224.          */
  225.         MsgInhibitPorts(output, alphaPosInput);
  226.         MsgConnectPorts(StringToAtom(betaData[ExecArg0].outName), input);
  227.  
  228.         while (MsgGetMsg())
  229.                 {
  230.                 if (IS_BETA_UP(betaPosition))
  231.                         MsgWriteToPort(output, ALPHA_INCR);
  232.                 }
  233.         }
  234.  
  235. /*      if the leg is down, move it up
  236.  */
  237. void uplegTrigger(void)
  238.         {
  239.         PORT betaPosition;
  240.         STRING_ATOM betaInput, betaOutput, output;
  241.  
  242.         betaInput = StringToAtom(betaData[ExecArg0].inName);
  243.         betaOutput = StringToAtom(betaData[ExecArg0].outName);
  244.  
  245.         output = StringToAtom(uplegData[ExecArg0].outName);
  246.         MsgSuppressPorts(output, betaInput);
  247.         MsgWatchTrigger(StringToAtom(uplegData[ExecArg0].inName));
  248.  
  249.         while (MsgGetMsg())
  250.                 {
  251.                 MsgPeekPort(betaOutput, &betaPosition);
  252.                 if (IS_BETA_DOWN(betaPosition))
  253.                         MsgWriteToPort(output, BETA_UP);
  254.                 }
  255.         }
  256.  
  257. void TripodGait(void)
  258.         {
  259.         STRING_ATOM evenCycleID, oddCycleID;
  260.         int cycle;
  261.  
  262.         evenCycleID = StringToAtom("walk.even");
  263.         oddCycleID = StringToAtom("walk.odd");
  264.  
  265.         /*      alternately raise the legs in a tripod gait fashion
  266.          */
  267.         MsgConnectPorts(evenCycleID, StringToAtom(uplegData[0].inName));
  268.         MsgConnectPorts(evenCycleID, StringToAtom(uplegData[2].inName));
  269.         MsgConnectPorts(evenCycleID, StringToAtom(uplegData[4].inName));
  270.         MsgConnectPorts(oddCycleID, StringToAtom(uplegData[1].inName));
  271.         MsgConnectPorts(oddCycleID, StringToAtom(uplegData[3].inName));
  272.         MsgConnectPorts(oddCycleID, StringToAtom(uplegData[5].inName));
  273.  
  274.         MsgWatchTimer(GAIT_CYCLE_TIME);
  275.         while (MsgGetMsg())
  276.                 {
  277.                 if (cycle)
  278.                         MsgWriteToPort(oddCycleID, 0);
  279.                 else
  280.                         MsgWriteToPort(evenCycleID, 0);
  281.                 cycle ^= 1;
  282.                 }
  283.         }
  284.  
  285. #ifndef HC11
  286. void nil(void)
  287.         {
  288.         MsgWatchTimer(4);
  289.         while (MsgGetMsg())
  290.                 if (kbhit())
  291.                         {
  292.                         if (getchar() == 'q')
  293.                                 exit(0);
  294.                         }
  295.         }
  296. #endif
  297.  
  298. #define SSIZE   100
  299. #define XSIZE    100
  300. #define BSIZE   100
  301.  
  302. int main()
  303.         {
  304.         void printServos();
  305.  
  306.         ExecInit();
  307.         ExecCreateThread(NORMAL_PROC, alphaPos, XSIZE, 0, 0);
  308.         ExecCreateThread(NORMAL_PROC, alphaPos, XSIZE, 1, 0);
  309.         ExecCreateThread(NORMAL_PROC, alphaPos, XSIZE, 2, 0);
  310.         ExecCreateThread(NORMAL_PROC, alphaPos, XSIZE, 3, 0);
  311.         ExecCreateThread(NORMAL_PROC, alphaPos, XSIZE, 4, 0);
  312.         ExecCreateThread(NORMAL_PROC, alphaPos, XSIZE, 5, 0);
  313.  
  314.         ExecCreateThread(NORMAL_PROC, betaPos, BSIZE, 0, 0);
  315.         ExecCreateThread(NORMAL_PROC, betaPos, BSIZE, 1, 0);
  316.         ExecCreateThread(NORMAL_PROC, betaPos, BSIZE, 2, 0);
  317.         ExecCreateThread(NORMAL_PROC, betaPos, BSIZE, 3, 0);
  318.         ExecCreateThread(NORMAL_PROC, betaPos, BSIZE, 4, 0);
  319.         ExecCreateThread(NORMAL_PROC, betaPos, BSIZE, 5, 0);
  320.  
  321.         ExecCreateThread(NORMAL_PROC, legDown, SSIZE, 0, 0);
  322.         ExecCreateThread(NORMAL_PROC, legDown, SSIZE, 1, 0);
  323.         ExecCreateThread(NORMAL_PROC, legDown, SSIZE, 2, 0);
  324.         ExecCreateThread(NORMAL_PROC, legDown, SSIZE, 3, 0);
  325.         ExecCreateThread(NORMAL_PROC, legDown, SSIZE, 4, 0);
  326.         ExecCreateThread(NORMAL_PROC, legDown, SSIZE, 5, 0);
  327.  
  328.         ExecCreateThread(NORMAL_PROC, alphaBalance, BSIZE, 0, 0);
  329.  
  330.         ExecCreateThread(NORMAL_PROC, alphaAdvance, SSIZE, 0, 0);
  331.         ExecCreateThread(NORMAL_PROC, alphaAdvance, SSIZE, 1, 0);
  332.         ExecCreateThread(NORMAL_PROC, alphaAdvance, SSIZE, 2, 0);
  333.         ExecCreateThread(NORMAL_PROC, alphaAdvance, SSIZE, 3, 0);
  334.         ExecCreateThread(NORMAL_PROC, alphaAdvance, SSIZE, 4, 0);
  335.         ExecCreateThread(NORMAL_PROC, alphaAdvance, SSIZE, 5, 0);
  336.  
  337.         ExecCreateThread(NORMAL_PROC, uplegTrigger, SSIZE, 0, 0);
  338.         ExecCreateThread(NORMAL_PROC, uplegTrigger, SSIZE, 1, 0);
  339.         ExecCreateThread(NORMAL_PROC, uplegTrigger, SSIZE, 2, 0);
  340.         ExecCreateThread(NORMAL_PROC, uplegTrigger, SSIZE, 3, 0);
  341.         ExecCreateThread(NORMAL_PROC, uplegTrigger, SSIZE, 4, 0);
  342.         ExecCreateThread(NORMAL_PROC, uplegTrigger, SSIZE, 5, 0);
  343.  
  344.         ExecCreateThread(NORMAL_PROC, TripodGait, SSIZE, 0, 0);
  345. #ifndef HC11
  346.         ExecCreateThread(NORMAL_PROC, nil, 200, 0, 0);
  347.         ExecCreateThread(NORMAL_PROC, printServos, 600, 0, 0);
  348. #endif
  349.         ExecStart();
  350.         }
  351.  
  352. - - --- end ---
  353.  
  354.